package pl.elka.simulator.core.mobilityManager.impl;

import java.util.ArrayList;

import pl.elka.simulator.core.AreaConstraint;
import pl.elka.simulator.core.CollisionSolver;
import pl.elka.simulator.core.EnviromentConfiguration;
import pl.elka.simulator.core.Node;
import pl.elka.simulator.core.Position;
import pl.elka.simulator.core.experiment.ConsistencyRaitingCounter;
import pl.elka.simulator.core.mobilityManager.IMobilityManager;
import pl.elka.simulator.core.mobilityManager.MobilityManagerType;
import pl.elka.simulator.core.mobilityManager.impl.nodes.RWMNode;

public class FlockMobilityModel extends RandomWalkMobilityManager {

	private Node[] localGroupNodes;
	private Position flockTarget;
	private AreaConstraint areaConstraint;
	private int timeFromTargetChange;
	private final int timeToTargetChange = 20;
	
	private final float BEST_NEIGHBOUR_DISTANCE = ConsistencyRaitingCounter.NEIGHBOUR_DISTANCE_SQRT/4;//polowa zasiegu
	final float keepDistanceFromNeighbourPriority = 8;
	final float moveToTargetPriority = 1f; 
	final float consistencePriority = 5;
	
	@Override
	public IMobilityManager newInstance() {		
		return new FlockMobilityModel();
	}
	@Override
	public MobilityManagerType getType() {
		return MobilityManagerType.FlockMobility;
	}
	@Override
	protected int randomInterval(Node n){
		return 1;
	}
	@Override
	public void applyEnviroment(EnviromentConfiguration cfg) {
		nodes = new ArrayList<RWMNode>();
		
		for(Node n : cfg.getNodes()){
			RWMNode rwmNode = new RWMNode(randomInterval(n), 0, new Position(0, 0), (Position)n.getPosition().clone());
			nodes.add(rwmNode);
			rwmNode.path.addPathElement(rwmNode.position.clone());
		}	
		constraints = cfg.getConstraints();
		areaConstraint = cfg.getAreaConstraint();
		
		collisionSolver = new CollisionSolver(cfg);
		
		localGroupNodes = new Node[nodes.size()];
		flockTarget = null;
		areaConstraint = cfg.getAreaConstraint();
		timeFromTargetChange = 0;
	}
	@Override
	public void makeMove(long elapsedTime) {
		doTargetThings(elapsedTime);
		for(RWMNode node:nodes){
			//wyznaczam nowa pozycje wezla
			Position newPosition = new Position(
						node.position.x + node.currentVelocity.x*elapsedTime,
						node.position.y+node.currentVelocity.y*elapsedTime
					);
			
			//sprawdzam, czy wezel, przy pokonywaniu drogi nie wpadnie na ograniczenie
			Position collisionPosition = collisionSolver.findCollisionPosition(node.position, newPosition);
			
			//jesli wpasl na ograniczenie, to ustawiam go przy nim i wymuszam zniame predkosci
			if(collisionPosition!=null){
				node.position = collisionPosition;
			}	
			//jesli nie wpadl, to po prostu go przestawiam
			else{
				node.position = newPosition;				
			}
			node.path.addPathElement(node.position.clone());
									
			node.currentVelocity = newVelocity(node);		
			
		}
	}
	private void doTargetThings(long elapsedTime) {
		timeFromTargetChange+=elapsedTime;
		if(timeFromTargetChange>=timeToTargetChange){
			timeFromTargetChange = 0;
			flockTarget = null;
		}
		if(flockTarget == null){
			flockTarget = randomTarget();
		}
	}
	public Position randomTarget(){		
		float x = (float)Math.random()*(areaConstraint.maxX - areaConstraint.minX) + areaConstraint.minX;
		float y = (float)Math.random()*(areaConstraint.maxY - areaConstraint.minY) + areaConstraint.minY;
		
		return new Position(x, y);
	}
	
	protected Position newVelocity(Node node){
		for(int i=0;i<localGroupNodes.length;++i){
			localGroupNodes[i] = null;
		}
		
		int localGroupCounter = 0;
		float nearestNodeDistanceSqrt = Float.MAX_VALUE;
		Node nearestNode = null;
		
		for(Node n:nodes){
			if(n!=node){
				float distSqrt = node.position.distSqrt(n.position);
				if(distSqrt<=ConsistencyRaitingCounter.NEIGHBOUR_DISTANCE_SQRT){
					localGroupNodes[localGroupCounter++] = n;
					if(distSqrt<nearestNodeDistanceSqrt){
						nearestNodeDistanceSqrt = distSqrt;
						nearestNode = n;
					}
				}
			}
		}
		
		Position velocity = new Position(0,0);

		moveToFlockTarget(node,velocity);
		if(nearestNode!=null){
			keepDistanceFromNeighbour(node,velocity,nearestNode);
			//keepFlockConsistence(velocity);
			moveAsNeighbour(node,velocity);
		}
		velocity.setMagnitude(Node.MAX_VELOCITY);
		return velocity;
	}
	private void keepFlockConsistence(Position velocity) {
		// TODO Auto-generated method stub
		
	}
	private void moveAsNeighbour(Node node,Position velocity) {
		Position avgVel = new Position(0,0);
		int localGroupSize = 0;
		for(Node n:localGroupNodes){
			if(n==null)
				break;
			avgVel.add(((RWMNode)n).currentVelocity);
			++localGroupSize;
		}
		if(localGroupSize==0)
			return;
		avgVel.div(localGroupSize);
		Position dif = new Position(avgVel.x-((RWMNode)node).currentVelocity.x,avgVel.y-((RWMNode)node).currentVelocity.y);
		if(dif.countLenSqrt()==0)
			return;
		dif.setMagnitude(consistencePriority*dif.countLenSqrt());		
		velocity.add(dif);
		
	}
	private void keepDistanceFromNeighbour(Node node,Position velocity, Node nearestNode) {		
		Position dif = new Position(nearestNode.position.x - node.position.x,nearestNode.position.y - node.position.y);
		if(dif.countLenSqrt()==0)
			return;
		dif.setMagnitude(keepDistanceFromNeighbourPriority*(Math.abs(dif.countLenSqrt() - BEST_NEIGHBOUR_DISTANCE)));
		//System.out.println("dif.countLenSqrt(): " + dif.countLenSqrt());
		if(dif.countLenSqrt()>BEST_NEIGHBOUR_DISTANCE){//wezel jest za daleko
			velocity.add(dif);
		}
		else{
			velocity.minus(dif);
		}		
		
	}
	private void moveToFlockTarget(Node node, Position velocity) {		
		Position dif = new Position(flockTarget.x - node.position.x,flockTarget.y - node.position.y);
		dif.setMagnitude(moveToTargetPriority);
		velocity.add(dif);
		
	}
}
